﻿#ifndef BUILD_OCC

#include <Geom2d_BezierCurve.hxx>
#include <Geom_BezierCurve.hxx>
#include <Geom_BezierSurface.hxx>
#include <TColStd_Array1OfInteger.hxx>
#include <TColStd_Array1OfReal.hxx>
#include <TColStd_Array2OfReal.hxx>
#include <TColgp_Array1OfPnt.hxx>
#include <TColgp_Array1OfPnt2d.hxx>
#include <TColgp_Array2OfPnt.hxx>

#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>
#include <xy/alg/distance.h>
#include <xy/geom/bezier_curve.h>
#include <xy/geom/bezier_surface.h>

using namespace xy::geom;
using namespace xy::alg;

class TestGeomBezier : public ::testing::Test
{
  protected:
    std::mt19937 m_random_engine;
    std::uniform_real_distribution<double> m_random_distribution{0, 10};

    void SetUp() override
    {
        m_random_engine.seed(std::random_device{}());
    }

    void TearDown() override {
        // nothing
    };

  public:
    std::tuple<Handle(Geom_BezierCurve), std::unique_ptr<bezier_curve<3>>> GenerateBezierCurve(int n)
    {
        TColgp_Array1OfPnt points(1, n);
        std::vector<vec3> poles(n);
        for (int i = 0; i < n; i++)
        {
            poles[i] = vec3(m_random_distribution(m_random_engine), m_random_distribution(m_random_engine),
                            m_random_distribution(m_random_engine));
            points.SetValue(i + 1, gp_Pnt(poles[i].x(), poles[i].y(), poles[i].z()));
        }

        Handle(Geom_BezierCurve) occCurve = new Geom_BezierCurve(points);
        std::unique_ptr<bezier_curve<3>> curve(new bezier_curve<3>(std::move(poles), n - 1));
        return {occCurve, std::move(curve)};
    }

    std::tuple<Handle(Geom_BezierCurve), std::unique_ptr<bezier_curve<3, true>>> GenerateBezierCurve_Weight(int n)
    {
        TColgp_Array1OfPnt points(1, n);
        std::vector<vec4> poles(n);
        for (int i = 0; i < n; i++)
        {
            poles[i] = vec4(m_random_distribution(m_random_engine), m_random_distribution(m_random_engine),
                            m_random_distribution(m_random_engine), m_random_distribution(m_random_engine));
            points.SetValue(i + 1, gp_Pnt(poles[i].x(), poles[i].y(), poles[i].z()));
        }

        TColStd_Array1OfReal weights(1, n);
        for (int i = 0; i < n; i++)
        {
            weights.SetValue(i + 1, poles[i].w());
        }

        Handle(Geom_BezierCurve) occCurve = new Geom_BezierCurve(points, weights);
        std::unique_ptr<bezier_curve<3, true>> curve(new bezier_curve<3, true>(std::move(poles), n - 1));
        return {occCurve, std::move(curve)};
    }

    std::tuple<Handle(Geom_BezierSurface), std::unique_ptr<bezier_surface<3>>> GenerateBezierSurface(int n, int m)
    {
        TColgp_Array2OfPnt points(1, n, 1, m);
        std::vector<vec3> poles(n * m);
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < m; j++)
            {
                int index = i * m + j;
                poles[index] = vec3(m_random_distribution(m_random_engine), m_random_distribution(m_random_engine),
                                    m_random_distribution(m_random_engine));
                points.SetValue(i + 1, j + 1, gp_Pnt(poles[index].x(), poles[index].y(), poles[index].z()));
            }
        }

        Handle(Geom_BezierSurface) occSurface = new Geom_BezierSurface(points);
        std::unique_ptr<bezier_surface<3>> surface(new bezier_surface<3>(std::move(poles), n - 1, m - 1));
        return {occSurface, std::move(surface)};
    }

    std::tuple<Handle(Geom_BezierSurface), std::unique_ptr<bezier_surface<3, true>>> GenerateBezierSurface_Weight(int n,
                                                                                                                  int m)
    {
        TColgp_Array2OfPnt points(1, n, 1, m);
        std::vector<vec4> poles(n * m);
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < m; j++)
            {
                int index = i * m + j;
                poles[index] = vec4(m_random_distribution(m_random_engine), m_random_distribution(m_random_engine),
                                    m_random_distribution(m_random_engine), m_random_distribution(m_random_engine));
                points.SetValue(i + 1, j + 1, gp_Pnt(poles[index].x(), poles[index].y(), poles[index].z()));
            }
        }
        TColStd_Array2OfReal weights(1, n, 1, m);
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < m; j++)
            {
                int index = i * m + j;
                weights.SetValue(i + 1, j + 1, poles[index].w());
            }
        }

        Handle(Geom_BezierSurface) occSurface = new Geom_BezierSurface(points, weights);
        std::unique_ptr<bezier_surface<3, true>> surface(new bezier_surface<3, true>(std::move(poles), n - 1, m - 1));
        return {occSurface, std::move(surface)};
    }
};

TEST_F(TestGeomBezier, curve)
{
    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve(N);

        int M = 1000;
        for (int i = 0; i < M; i++)
        {
            double t = i / (M - 1.0);
            gp_Pnt occP;
            gp_Vec occD1, occD2, occD3;
            occCurve->D3(t, occP, occD1, occD2, occD3);

            auto [p, d1, d2] = curve->d2(t);
            vec3 diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
            vec3 diffD1 = d1 - vec3(occD1.X(), occD1.Y(), occD1.Z());
            vec3 diffD2 = d2 - vec3(occD2.X(), occD2.Y(), occD2.Z());

            EXPECT_LT((curve->d0(t) - p).modulus(), 1e-6);
            EXPECT_LT(diffP.modulus(), 1e-6);
            EXPECT_LT(diffD1.modulus(), 1e-6);
            EXPECT_LT(diffD2.modulus(), 1e-6);
        }

        auto [first, last] = curve->bound(0);
        EXPECT_DOUBLE_EQ(occCurve->FirstParameter(), first);
        EXPECT_DOUBLE_EQ(occCurve->LastParameter(), last);
        EXPECT_FALSE(curve->is_rational());
        EXPECT_EQ(occCurve->Degree(), curve->degree());
        EXPECT_EQ(occCurve->NbPoles(), curve->pole_num());
    }

    // 测试修改控制点
    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve(N);

        for (int i = 0; i < curve->pole_num(); i++)
        {
            curve->set_pole(vec3(m_random_distribution(m_random_engine), m_random_distribution(m_random_engine),
                                 m_random_distribution(m_random_engine)),
                            i);
            occCurve->SetPole(i + 1, gp_Pnt(curve->pole(i).x(), curve->pole(i).y(), curve->pole(i).z()));
        }

        int M = 1000;
        for (int i = 0; i < M; i++)
        {
            double t = i / (M - 1.0);
            gp_Pnt occP;
            gp_Vec occD1, occD2, occD3;
            occCurve->D3(t, occP, occD1, occD2, occD3);

            auto [p, d1, d2] = curve->d2(t);
            vec3 diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
            vec3 diffD1 = d1 - vec3(occD1.X(), occD1.Y(), occD1.Z());
            vec3 diffD2 = d2 - vec3(occD2.X(), occD2.Y(), occD2.Z());

            EXPECT_LT((curve->d0(t) - p).modulus(), 1e-6);
            EXPECT_LT(diffP.modulus(), 1e-6);
            EXPECT_LT(diffD1.modulus(), 1e-6);
            EXPECT_LT(diffD2.modulus(), 1e-6);
        }

        auto [first, last] = curve->bound(0);
        EXPECT_DOUBLE_EQ(occCurve->FirstParameter(), first);
        EXPECT_DOUBLE_EQ(occCurve->LastParameter(), last);
        EXPECT_FALSE(curve->is_rational());
        EXPECT_EQ(occCurve->Degree(), curve->degree());
        EXPECT_EQ(occCurve->NbPoles(), curve->pole_num());
    }

    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve_Weight(N);

        int M = 1000;
        for (int i = 0; i < M; i++)
        {
            double t = i / (M - 1.0);
            gp_Pnt occP;
            gp_Vec occD1, occD2, occD3;
            occCurve->D3(t, occP, occD1, occD2, occD3);

            auto [p, d1, d2] = curve->d2(t);
            vec3 diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
            vec3 diffD1 = d1 - vec3(occD1.X(), occD1.Y(), occD1.Z());
            vec3 diffD2 = d2 - vec3(occD2.X(), occD2.Y(), occD2.Z());

            EXPECT_LT((curve->d0(t) - p).modulus(), 1e-6);
            EXPECT_LT(diffP.modulus(), 1e-6);
            EXPECT_LT(diffD1.modulus(), 1e-6);
            EXPECT_LT(diffD2.modulus(), 1e-6);
        }

        auto [first, last] = curve->bound(0);
        EXPECT_DOUBLE_EQ(occCurve->FirstParameter(), first);
        EXPECT_DOUBLE_EQ(occCurve->LastParameter(), last);
        EXPECT_TRUE(curve->is_rational());
        EXPECT_EQ(occCurve->Degree(), curve->degree());
        EXPECT_EQ(occCurve->NbPoles(), curve->pole_num());
    }

    // 测试修改权重
    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve_Weight(N);

        for (int i = 0; i < curve->pole_num(); i++)
        {
            curve->set_weight(m_random_distribution(m_random_engine), i);
            occCurve->SetWeight(i + 1, curve->weight(i));
        }

        int M = 1000;
        for (int i = 0; i < M; i++)
        {
            double t = i / (M - 1.0);
            gp_Pnt occP;
            gp_Vec occD1, occD2, occD3;
            occCurve->D3(t, occP, occD1, occD2, occD3);

            auto [p, d1, d2] = curve->d2(t);
            vec3 diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
            vec3 diffD1 = d1 - vec3(occD1.X(), occD1.Y(), occD1.Z());
            vec3 diffD2 = d2 - vec3(occD2.X(), occD2.Y(), occD2.Z());

            EXPECT_LT((curve->d0(t) - p).modulus(), 1e-6);
            EXPECT_LT(diffP.modulus(), 1e-6);
            EXPECT_LT(diffD1.modulus(), 1e-6);
            EXPECT_LT(diffD2.modulus(), 1e-6);
        }

        auto [first, last] = curve->bound(0);
        EXPECT_DOUBLE_EQ(occCurve->FirstParameter(), first);
        EXPECT_DOUBLE_EQ(occCurve->LastParameter(), last);
        EXPECT_TRUE(curve->is_rational());
        EXPECT_EQ(occCurve->Degree(), curve->degree());
        EXPECT_EQ(occCurve->NbPoles(), curve->pole_num());
    }

    // 测试升降阶
    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve_Weight(N);

        for (int i = 0; i < 2; i++)
        {
            // 先升 1 次
            curve->elevate(curve->degree() + 1);

            int M = 1000;
            for (int i = 0; i < M; i++)
            {
                double t = i / (M - 1.0);
                gp_Pnt occP;
                gp_Vec occD1, occD2, occD3;
                occCurve->D3(t, occP, occD1, occD2, occD3);

                auto [p, d1, d2] = curve->d2(t);
                vec3 diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
                vec3 diffD1 = d1 - vec3(occD1.X(), occD1.Y(), occD1.Z());
                vec3 diffD2 = d2 - vec3(occD2.X(), occD2.Y(), occD2.Z());

                EXPECT_LT((curve->d0(t) - p).modulus(), 1e-6);
                EXPECT_LT(diffP.modulus(), 1e-6);
                EXPECT_LT(diffD1.modulus(), 1e-6);
                EXPECT_LT(diffD2.modulus(), 1e-6);
            }
        }

        // 再降次，得到误差界
        for (int i = 0; i < 2; i++)
        {
            std::vector<vec4> poles;
            for (int i = 0; i < curve->pole_num(); i++)
            {
                vec4 pole;
                pole.x() = curve->pole(i).x();
                pole.y() = curve->pole(i).y();
                pole.z() = curve->pole(i).z();
                pole.w() = curve->weight(i);
                poles.push_back(pole);
            }
            std::shared_ptr<bezier_curve<3, true>> curve2(new bezier_curve<3, true>(std::move(poles), curve->degree()));

            double bound = curve->reduce();
            std::cout << "误差界：" << bound << std::endl;

            int M = 100;
            for (int i = 0; i < M; i++)
            {
                double t = i / (M - 1.0);
                vec3 diffP = curve->d0(t) - curve2->d0(t);

                EXPECT_LT(diffP.modulus(), bound);
            }
        }
    }

    // 测试分割算法
    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve(N);
        auto [cpt1, cpt2] = curve->split(0.75);
        auto c1 = std::make_shared<bezier_curve<3>>(std::move(cpt1), N - 1);
        auto c2 = std::make_shared<bezier_curve<3>>(std::move(cpt2), N - 1);

        int M = 100;
        for (int i = 0; i < M; i++)
        {
            double t = i / (M - 1.0);
            vec3 p;
            if (t < 0.75)
                p = c1->d0(t / 0.75);
            else
                p = c2->d0((t - 0.75) / 0.25);

            gp_Pnt occP;
            occCurve->D0(t, occP);

            auto diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
            EXPECT_LT(diffP.modulus(), 1e-6);
        }
    }

    // 测试反转
    {
        int N = 10;
        auto [occCurve, curve] = GenerateBezierCurve(N);
        curve->reverse();

        int M = 100;
        for (int i = 0; i < M; i++)
        {
            double t = i / (M - 1.0);
            vec3 p = curve->d0(1.0 - t);

            gp_Pnt occP;
            occCurve->D0(t, occP);

            auto diffP = p - vec3(occP.X(), occP.Y(), occP.Z());
            EXPECT_LT(diffP.modulus(), 1e-6);
        }
    }
}

TEST_F(TestGeomBezier, surface)
{
    for (int i = 0; i < 4; i++)
    {
        int Nu = 10, Nv = 8;
        auto [occSurface, surface] = GenerateBezierSurface(Nu, Nv);

        for (int i = 0; i < Nu; i++)
        {
            surface->pole(i, 4) = vec3(m_random_distribution(m_random_engine), m_random_distribution(m_random_engine),
                                       m_random_distribution(m_random_engine));
            occSurface->SetPole(i + 1, 5,
                                gp_Pnt(surface->pole(i, 4).x(), surface->pole(i, 4).y(), surface->pole(i, 4).z()));
        }

        int M = 100;
        for (int i = 0; i < M; i++)
        {
            double u = i / (M - 1.0);
            for (int j = 0; j < M; j++)
            {
                double v = j / (M - 1.0);
                auto [P, Du, Dv, Duu, Dvv, Duv] = surface->d2({u, v});

                gp_Pnt occP;
                gp_Vec occDu, occDv, occDuu, occDvv, occDuv;
                occSurface->D2(u, v, occP, occDu, occDv, occDuu, occDvv, occDuv);

                vec3 diffP = P - vec3(occP.X(), occP.Y(), occP.Z());
                vec3 diffDu = Du - vec3(occDu.X(), occDu.Y(), occDu.Z());
                vec3 diffDv = Dv - vec3(occDv.X(), occDv.Y(), occDv.Z());
                vec3 diffDuu = Duu - vec3(occDuu.X(), occDuu.Y(), occDuu.Z());
                vec3 diffDvv = Dvv - vec3(occDvv.X(), occDvv.Y(), occDvv.Z());
                vec3 diffDuv = Duv - vec3(occDuv.X(), occDuv.Y(), occDuv.Z());

                EXPECT_LT((surface->d0({u, v}) - P).modulus(), 1e-6);
                EXPECT_LT(diffP.modulus(), 1e-6);
                EXPECT_LT(diffDu.modulus(), 1e-6);
                EXPECT_LT(diffDv.modulus(), 1e-6);
                EXPECT_LT(diffDuu.modulus(), 1e-6);
                EXPECT_LT(diffDvv.modulus(), 1e-6);
                EXPECT_LT(diffDuv.modulus(), 1e-6);
            }
        }

        auto [U1, U2] = surface->bound(0);
        auto [V1, V2] = surface->bound(1);

        EXPECT_TRUE(U1 == 0.0);
        EXPECT_TRUE(U2 == 1.0);
        EXPECT_TRUE(V1 == 0.0);
        EXPECT_TRUE(V2 == 1.0);
        EXPECT_FALSE(surface->is_rational());
        EXPECT_EQ(occSurface->NbUPoles(), surface->pole_num(0));
        EXPECT_EQ(occSurface->NbVPoles(), surface->pole_num(1));
        EXPECT_EQ(occSurface->UDegree(), surface->degree(0));
        EXPECT_EQ(occSurface->VDegree(), surface->degree(1));

        std::swap(Nu, Nv);
    }

    for (int i = 0; i < 4; i++)
    {
        int Nu = 10, Nv = 8;
        auto [occSurface, surface] = GenerateBezierSurface_Weight(Nu, Nv);

        for (int i = 0; i < Nu; i++)
        {
            surface->set_weight(m_random_distribution(m_random_engine), i, 4);
            occSurface->SetWeight(i + 1, 5, surface->weight(i, 4));
        }

        int M = 100;
        for (int i = 0; i < M; i++)
        {
            double u = i / (M - 1.0);
            for (int j = 0; j < M; j++)
            {
                double v = j / (M - 1.0);
                auto [P, Du, Dv, Duu, Dvv, Duv] = surface->d2({u, v});

                gp_Pnt occP;
                gp_Vec occDu, occDv, occDuu, occDvv, occDuv;
                occSurface->D2(u, v, occP, occDu, occDv, occDuu, occDvv, occDuv);

                vec3 diffP = P - vec3(occP.X(), occP.Y(), occP.Z());
                vec3 diffDu = Du - vec3(occDu.X(), occDu.Y(), occDu.Z());
                vec3 diffDv = Dv - vec3(occDv.X(), occDv.Y(), occDv.Z());
                vec3 diffDuu = Duu - vec3(occDuu.X(), occDuu.Y(), occDuu.Z());
                vec3 diffDvv = Dvv - vec3(occDvv.X(), occDvv.Y(), occDvv.Z());
                vec3 diffDuv = Duv - vec3(occDuv.X(), occDuv.Y(), occDuv.Z());

                EXPECT_LT((surface->d0({u, v}) - P).modulus(), 1e-6);
                EXPECT_LT(diffP.modulus(), 1e-6);
                EXPECT_LT(diffDu.modulus(), 1e-6);
                EXPECT_LT(diffDv.modulus(), 1e-6);
                EXPECT_LT(diffDuu.modulus(), 1e-6);
                EXPECT_LT(diffDvv.modulus(), 1e-6);
                EXPECT_LT(diffDuv.modulus(), 1e-6);
            }
        }

        auto [U1, U2] = surface->bound(0);
        auto [V1, V2] = surface->bound(1);

        EXPECT_TRUE(U1 == 0.0);
        EXPECT_TRUE(U2 == 1.0);
        EXPECT_TRUE(V1 == 0.0);
        EXPECT_TRUE(V2 == 1.0);
        EXPECT_TRUE(surface->is_rational());
        EXPECT_EQ(occSurface->NbUPoles(), surface->pole_num(0));
        EXPECT_EQ(occSurface->NbVPoles(), surface->pole_num(1));
        EXPECT_EQ(occSurface->UDegree(), surface->degree(0));
        EXPECT_EQ(occSurface->VDegree(), surface->degree(1));

        std::swap(Nu, Nv);
    }
}

TEST_F(TestGeomBezier, quadric)
{
    // 二次有理 Bezier 曲线
    {
        auto qcurve =
            std::make_shared<bezier_curve_qr<3>>(vec3{0.0, 0.0, 0.0}, vec4{0.0, 1.0, 0.0, 0.5}, vec3{1.0, 1.0, 0.0});
    }

    // 带有无穷远点的半圆
    {
        auto qcurve =
            std::make_shared<bezier_curve_qr<3>>(vec3{0.0, 0.0, 0.0}, vec4{0.0, 0.0, 0.0, 0.0}, vec3{2.0, 0.0, 0.0});
        qcurve->set_inf_pole(vec4{0.0, 1.0, 0.0, 0.0}, 1);
    }

    // 构造单个圆弧
    {
        // 非无穷控制点
        auto curve1 = bezier_curve_qr<3>::make_one_arc(vec3{0.0, 0.0, 0.0}, vec3{0.0, 0.0, 1.0}, vec3{1.0, 0.0, 0.0},
                                                       vec3{1.0, 0.0, 1.0}, vec3{0.5, 0.0, 1.0});

        // 无穷控制点
        auto curve2 = bezier_curve_qr<3>::make_one_arc(vec3{0.0, 0.0, 0.0}, vec3{0.0, 0.0, 1.0}, vec3{1.0, 0.0, 0.0},
                                                       vec3{1.0, 0.0, 0.0}, vec3{0.5, 0.0, 1.0});
    }

    // 单个圆弧在肩点处切割
    {
        auto curve = bezier_curve_qr<3>::make_one_arc(vec3{0.0, 0.0, 0.0}, vec3{-1.0, 0.0, 1.0}, vec3{1.0, 0.0, 0.0},
                                                      vec3{0.0, 0.0, -1.0}, vec3{0.5, 0.0, 1.0});
        auto [P1, P2] = curve->split_at_shoulder_point();
        auto c1 = std::make_shared<bezier_curve_qr<3>>(P1[0], P1[1], P1[2]);
        auto c2 = std::make_shared<bezier_curve_qr<3>>(P2[0], P2[1], P2[2]);

        auto [Q, R] = curve->split(0.5);
        auto q1 = std::make_shared<bezier_curve_qr<3>>(Q[0], Q[1], Q[2]);
        auto q2 = std::make_shared<bezier_curve_qr<3>>(R[0], R[1], R[2]);

        std::cout << "distance1: " << distance_metric<3>::distance(c1, q1) << std::endl;
        std::cout << "distance2: " << distance_metric<3>::distance(c2, q2) << std::endl;
    }

    {
        auto curve = bezier_curve_qr<3>::make_one_arc(vec3{0.0, 0.0, 0.0}, vec3{0.0, 0.0, 1.0}, vec3{1.0, 0.0, 0.0},
                                                      vec3{0.0, 0.0, -1.0}, vec3{0.5, 0.0, 1.0});
        auto [P1, P2] = curve->split_at_shoulder_point();
        auto c1 = std::make_shared<bezier_curve_qr<3>>(P1[0], P1[1], P1[2]);
        auto c2 = std::make_shared<bezier_curve_qr<3>>(P2[0], P2[1], P2[2]);

        auto [Q, R] = curve->split(0.5);
        auto q1 = std::make_shared<bezier_curve_qr<3>>(Q[0], Q[1], Q[2]);
        auto q2 = std::make_shared<bezier_curve_qr<3>>(R[0], R[1], R[2]);

        std::cout << "distance1: " << distance_metric<3>::distance(c1, q1) << std::endl;
        std::cout << "distance2: " << distance_metric<3>::distance(c2, q2) << std::endl;
    }
}

#endif
